#include "GSStm32F4_includes.h"
_ARM_IK_DATA g_ikData;

static _FLOAT classic6dof_DH[5][4] = {
	{	0.0f,		50.0f,	    0,	        (_FLOAT)M_PI_2  },
	{	0.0f,	    0.0f,	    120.0f,	    0.0f            },
	{	0.0f,	    0.0f,	    120.0f,		0.0f	        },
	{	0.0f,		0.0f,	    29.25f,		-(_FLOAT)M_PI_2	},
	{	0.0f,		0.0f,	    150.0f,		0.0f	        }
}; // home, d, a, alpha


void s_setdDhHome(_FLOAT * l_dh){
    *(l_dh)    =-(_FLOAT)M_PI_2;
    *(l_dh +=4)=0.16f;
    *(l_dh +=4)=(_FLOAT)0.22;
    *(l_dh +=4)=0.35f;
    *(l_dh +=4)=(_FLOAT)0.5f;
}

void s_getLinkTransform(_FLOAT * l_link, _FLOAT * l_output){
    _FLOAT l_th, l_d, l_a, l_al;
    _FLOAT l_cosTh,l_sinTh,l_cosAl,l_sinAl;
    l_th     = *l_link;
    l_d      = *(l_link+1);
    l_a      = *(l_link+2);
    l_al     = *(l_link+3);

    l_cosTh  = cosf(l_th);
    l_sinTh  = sinf(l_th);
    l_cosAl  = cosf(l_al);
    l_sinAl  = sinf(l_al);

    *(l_output ++) = l_cosTh            ;
    *(l_output ++) = -l_sinTh*l_cosAl   ;
    *(l_output ++) = l_sinTh*l_sinAl    ;
    *(l_output ++) = l_a*l_cosTh        ;

    *(l_output ++) = l_sinTh            ;
    *(l_output ++) = l_cosTh*l_cosAl    ;
    *(l_output ++) = -l_cosTh*l_sinAl   ;
    *(l_output ++) = l_a*l_sinTh        ;

    *(l_output ++) = 0.0f               ;
    *(l_output ++) = l_sinAl            ;
    *(l_output ++) = l_cosAl            ;
    *(l_output ++) = l_d                ;   

    *(l_output ++) = 0.0f               ;
    *(l_output ++) = 0.0f               ;
    *(l_output ++) = 0.0f               ;
    *(l_output ) =  1.0f               ;
}

void g_FKCalMain(void){
    _FLOAT l_mtTemp1[4][4],l_mtTemp2[4][4];
    s_setdDhHome(classic6dof_DH[0]);
    s_getLinkTransform(classic6dof_DH[0], g_ikData.d_link1[0]);
    s_getLinkTransform(classic6dof_DH[1], g_ikData.d_link2[0]);
    s_getLinkTransform(classic6dof_DH[2], g_ikData.d_link3[0]);
    s_getLinkTransform(classic6dof_DH[3], g_ikData.d_link4[0]);
    s_getLinkTransform(classic6dof_DH[4], g_ikData.d_link5[0]);
    
    g_matMultiply(g_ikData.d_link1[0],g_ikData.d_link2[0],l_mtTemp1[0],4,4,4);
    g_matMultiply(l_mtTemp1[0],g_ikData.d_link3[0],l_mtTemp2[0],4,4,4);
    g_matMultiply(l_mtTemp2[0],g_ikData.d_link4[0],l_mtTemp1[0],4,4,4);
    g_matMultiply(l_mtTemp1[0],g_ikData.d_link5[0],g_ikData.d_linkTF[0],4,4,4);
}

void g_IKCalMain(void){
    _FLOAT l_tanc1;
  
    l_tanc1 = g_ikData.d_linkTF[1][2] / g_ikData.d_linkTF[0][2];
    g_ikData.d_angle[0] = atanf(l_tanc1);

    g_ikData.d_abc[0] = g_ikData.d_linkTF[0][0];
    g_ikData.d_abc[1] = g_ikData.d_linkTF[0][1];
    g_ikData.d_abc[2] = sinf(g_ikData.d_angle[0]);
    // g_getIKMath();
}

